package com.hongji.lms5xxcommunicator.scanner.imp;

import com.hongji.lms5xxcommunicator.realtime.sync.XSource;
import com.hongji.lms5xxcommunicator.scanner.ScanFrame;
import com.hongji.lms5xxcommunicator.scanner.ScanStrategy;
import com.hongji.lms5xxcommunicator.scanner.config.LivoxConfig;
import com.hongji.lms5xxcommunicator.scanner.config.LivoxConfig.MountSide;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Service;

import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetSocketAddress;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Date;
import java.util.Locale;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicLong;

/**
 * Livox Mid-360 策略（UDP 收点 + 完整报文解析 + 传感器→世界坐标 + 过滤 + 按开关进行 PLC-X 融合）
 */
@Slf4j
@Service
public class LivoxMid360ScannerStrategy implements ScanStrategy {

    /* ======================= 常量定义 ======================= */
    private static final int HEADER_SIZE = 36;   // Mid-360 UDP头长度（小端）
    private static final int DT_CART_32  = 1;    // 笛卡尔32位: x/y/z int32 (mm)
    private static final int DT_CART_16  = 2;    // 笛卡尔16位: x/y/z int16 (10mm)

    /* ======================= 依赖与状态 ======================= */
    private final XSource xSource;
    private final LivoxConfig cfg;

    private volatile ScanFrameListener listener;
    private final AtomicBoolean running = new AtomicBoolean(false);
    private volatile double lastX = 0.0;

    private DatagramSocket sock;
    private ExecutorService ioExec;

    /* ======================= 本地持久化（原样保留） ======================= */
    private volatile PrintWriter currentWriter = null;
    private final AtomicLong currentFileStartTime = new AtomicLong(0);
    private final AtomicLong pointCountInCurrentFile = new AtomicLong(0);
    private static final long FILE_SWITCH_INTERVAL_MS = 3000;
    private static final String OUTPUT_DIR = "./livox_raw_capture";
    private volatile boolean persistenceEnabled = true;
    private volatile long packetCounter = 0;

    /* ======================= Spring 构造 ======================= */
    @Autowired
    public LivoxMid360ScannerStrategy(
            XSource xSource,
            @Value("${scanner.Livox.udp.port:56301}") int udpPort,
            @Value("${scanner.Livox.udp.recv_buf_bytes:4194304}") int recvBufBytes,

            @Value("${scanner.Livox.offset.dot_num:5}") int offDotNum,
            @Value("${scanner.Livox.offset.data_type:10}") int offDataType,
            @Value("${scanner.Livox.header_bytes:18}") int headerBytes,

            @Value("${scanner.Livox.pt.stride.type1:12}") int strideType1,
            @Value("${scanner.Livox.pt.stride.type2:6}") int strideType2,
            @Value("${scanner.Livox.pt.skip_after_xyz.type1:0}") int skipAfter1,
            @Value("${scanner.Livox.pt.skip_after_xyz.type2:0}") int skipAfter2,

            @Value("${scanner.Livox.debug.hexdump:false}") boolean debugHexdump,

            // 过滤窗口（世界系）
            @Value("${scanner.Livox.filter.z_min:-9.9E9}") double zMin,
            @Value("${scanner.Livox.filter.z_max:9.9E9}")  double zMax,
            @Value("${grid.spec.y0:-9.9E9}") double yMin,
            @Value("${grid.spec.y1:9.9E9}")  double yMax,
            // ✅ 新增：X 过滤（沿用现有 grid.spec.x0/x1）
            @Value("${grid.spec.x0:-9.9E9}") double xMin,
            @Value("${grid.spec.x1:9.9E9}")  double xMax,

            // ===== 坐标统一（比例/翻转/平移）=====
            @Value("${scanner.coord.enabled:true}") boolean coordEnabled,
            @Value("${scanner.coord.x_scale:1.0}") double xScale,
            @Value("${scanner.coord.y_scale:1.0}") double yScale,
            @Value("${scanner.coord.z_scale:1.0}") double zScale,
            @Value("${scanner.coord.invert_x:false}") boolean invertX,
            @Value("${scanner.coord.invert_y:false}") boolean invertY,
            @Value("${scanner.coord.invert_z:false}") boolean invertZ,
            @Value("${scanner.coord.y_base:0}") double yBase,      // 兼容字段，可加到 yOffset
            @Value("${scanner.coord.z_offset:0}") double zOffset,  // 兼容字段，叠加到 zOffset

            // 安装外参/姿态（侧装/竖装皆可）
            @Value("${scanner.Livox.h0:0}") double h0,
            @Value("${scanner.Livox.x_offset:0}") double xOffset,
            @Value("${scanner.Livox.y_offset:0}") double yOffset,
            @Value("${scanner.Livox.sx:1}") int sX,
            @Value("${scanner.Livox.sy:1}") int sY,
            @Value("${scanner.Livox.sz:1}") int sZ,
            @Value("${scanner.Livox.mount.side:RIGHT}") String sideStr,
            @Value("${scanner.Livox.mount.yaw_deg:0}") double yawDeg,
            @Value("${scanner.Livox.mount.pitch_deg:0}") double pitchDeg,
            @Value("${scanner.Livox.mount.roll_deg:0}") double rollDeg,

            // 与 PLC-X 融合（开关）
            @Value("${x.lookup.tolerance.ms:30}") int xLookupTolMs,
            @Value("${scanner.Livox.plc.merge.enabled:false}") boolean plcMergeEnabled,

            // 持久化
            @Value("${scanner.Livox.persistence.enabled:true}") boolean persistenceEnabled
    ) {
        this.xSource = xSource;

        LivoxConfig c = new LivoxConfig();
        // UDP/解析
        c.udpPort = udpPort;
        c.recvBufBytes = Math.max(recvBufBytes, 1 << 20);
        c.offDotNum = offDotNum;
        c.offDataType = offDataType;
        c.headerBytes = headerBytes;
        c.strideType1 = strideType1;
        c.strideType2 = strideType2;
        c.skipAfter1 = skipAfter1;
        c.skipAfter2 = skipAfter2;
        c.debugHexdump = debugHexdump;

        // 过滤
        c.zMin = zMin; c.zMax = zMax;
        c.yMin = yMin; c.yMax = yMax;
        c.xMin = xMin; c.xMax = xMax;

        // 坐标统一
        c.coordEnabled = coordEnabled;
        c.xScale = xScale; c.yScale = yScale; c.zScale = zScale;
        c.invertX = invertX; c.invertY = invertY; c.invertZ = invertZ;

        c.xOffset = xOffset;
        c.yOffset = yOffset + yBase; // 兼容：y_base 叠加到 yOffset
        c.zOffset = zOffset;
        c.h0 = h0;
        c.sX = sX; c.sY = sY; c.sZ = sZ;

        // 姿态/侧别
        c.yawDeg = yawDeg;
        c.pitchDeg = pitchDeg;
        c.rollDeg = rollDeg;
        try { c.side = MountSide.valueOf(sideStr.trim().toUpperCase(Locale.ROOT)); }
        catch (Exception ignore){ c.side = MountSide.RIGHT; }

        // PLC-X 融合
        c.xLookupTolMs = xLookupTolMs;
        c.plcMergeEnabled = plcMergeEnabled;

        c.manageXSourceLifecycle = true;
        c.recomputeRotation();
        this.cfg = c;
        this.persistenceEnabled = persistenceEnabled;
    }

    /* 非 Spring 构造（多设备策略可直接传 LivoxConfig） */
    public LivoxMid360ScannerStrategy(XSource xSource, LivoxConfig cfg) {
        this.xSource = xSource;
        this.cfg = (cfg == null ? new LivoxConfig() : cfg);
        if (this.cfg.recvBufBytes < (1 << 20)) this.cfg.recvBufBytes = (1 << 20);
        this.cfg.recomputeRotation();
        this.persistenceEnabled = true;
    }

    /* ======================= 生命周期 ======================= */
    @Override
    public void start(ScanFrameListener listener) throws Exception {
        if (!running.compareAndSet(false, true)) return;
        this.listener = listener;

        if (persistenceEnabled) initializePersistence();
        if (cfg.manageXSourceLifecycle) xSource.start();

        sock = new DatagramSocket(null);
        sock.setReuseAddress(true);
        sock.setReceiveBufferSize(cfg.recvBufBytes);
        sock.bind(new InetSocketAddress("0.0.0.0", cfg.udpPort));

        ioExec = Executors.newSingleThreadExecutor(r -> {
            Thread t = new Thread(r, "livox-udp-io-" + cfg.udpPort);
            t.setDaemon(true);
            return t;
        });
        ioExec.submit(this::readLoop);

        log.info("[LIVOX] started cfg={}, persistence={}", cfg, persistenceEnabled);
    }

    @Override
    public void stop() {
        if (!running.getAndSet(false)) return;
        try { if (sock != null) sock.close(); } catch (Exception ignore) {}
        if (ioExec != null) ioExec.shutdownNow();
        closeCurrentWriter();
        try { if (cfg.manageXSourceLifecycle) xSource.stop(); } catch (Exception ignore) {}
        log.info("[LIVOX] stopped (udpPort={})", cfg.udpPort);
    }

    @Override public boolean isRunning() { return running.get(); }

    /* ======================= UDP 线程 ======================= */
    private void readLoop() {
        byte[] buf = new byte[65535];
        DatagramPacket pkt = new DatagramPacket(buf, buf.length);

        while (running.get()) {
            try {
                sock.receive(pkt);
                final long hostNs = System.nanoTime();
                packetCounter++;

                SensorPoints sp = parsePacketNew(pkt.getData(), pkt.getLength(), hostNs);
                if (sp == null || sp.count == 0) continue;

                if (persistenceEnabled) persistRawPointsToFile(sp, hostNs, packetCounter);

                final long tolNs = Math.max(1, cfg.xLookupTolMs) * 1_000_000L;
                final double plcX = cfg.plcMergeEnabled
                        ? xSource.nearestAtHostNs(hostNs, tolNs, lastX)
                        : 0.0; // 关闭融合时不查 OPC，减少干扰
                lastX = plcX;

                WorldPoints wp = mapToWorldAndFilter(sp, plcX);
                if (wp.valid == 0) continue;

                pushFrame(hostNs, wp);

            } catch (Throwable t) {
                if (running.get()) log.warn("[LIVOX] UDP read error: {}", t.toString());
            }
        }
    }

    /* ======================= 报文解析 ======================= */
    private SensorPoints parsePacketNew(byte[] data, int len, long hostNs) {
        if (len < HEADER_SIZE) return null;

        if (cfg.debugHexdump) hexdump(data, len, 36);

        ByteBuffer bb = ByteBuffer.wrap(data, 0, len).order(ByteOrder.LITTLE_ENDIAN);

        bb.get();                          // version
        int totalLen = bb.getShort() & 0xFFFF;
        bb.getShort();                     // timeInterval
        int dotNum = bb.getShort() & 0xFFFF;
        bb.getShort();                     // udpCnt
        bb.get();                          // frameCnt
        int dataType = bb.get() & 0xFF;
        bb.get();                          // timeType
        bb.get();                          // packInfo
        bb.position(bb.position() + 11);   // reserved
        bb.getInt();                       // crc32
        long packetTimestamp = bb.getLong(); // timestamp ns

        if (dotNum <= 0) return null;

        double[] xs = new double[dotNum];
        double[] ys = new double[dotNum];
        double[] zs = new double[dotNum];
        int[] intensities = new int[dotNum];
        int actualCount = 0;

        if (dataType == DT_CART_32) {
            final int recSize = 14; // x(i32 mm), y(i32 mm), z(i32 mm), refl(u8), tag(u8)
            int avail = Math.max(0, len - HEADER_SIZE);
            int n = Math.min(dotNum, avail / recSize);
            for (int i = 0; i < n; i++) {
                int x_mm = bb.getInt();
                int y_mm = bb.getInt();
                int z_mm = bb.getInt();
                int refl = bb.get() & 0xFF;
                bb.get(); // tag
                xs[actualCount] = x_mm;
                ys[actualCount] = y_mm;
                zs[actualCount] = z_mm;
                intensities[actualCount] = refl;
                actualCount++;
            }
        } else if (dataType == DT_CART_16) {
            final int recSize = 8; // x(i16 10mm), y(i16 10mm), z(i16 10mm), refl(u8), tag(u8)
            int avail = Math.max(0, len - HEADER_SIZE);
            int n = Math.min(dotNum, avail / recSize);
            for (int i = 0; i < n; i++) {
                short x_10mm = bb.getShort();
                short y_10mm = bb.getShort();
                short z_10mm = bb.getShort();
                int refl = bb.get() & 0xFF;
                bb.get(); // tag
                xs[actualCount] = x_10mm * 10.0;
                ys[actualCount] = y_10mm * 10.0;
                zs[actualCount] = z_10mm * 10.0;
                intensities[actualCount] = refl;
                actualCount++;
            }
        } else {
            if (cfg.debugHexdump) log.warn("[LIVOX] Unsupported data type: {}", dataType);
            return null;
        }

        if (actualCount == 0) return null;
        if (actualCount < dotNum) {
            xs = Arrays.copyOf(xs, actualCount);
            ys = Arrays.copyOf(ys, actualCount);
            zs = Arrays.copyOf(zs, actualCount);
            intensities = Arrays.copyOf(intensities, actualCount);
        }
        return new SensorPoints(xs, ys, zs, intensities, actualCount, packetTimestamp);
    }

    /* ======================= 传感器→世界 + PLC-X 融合 + 过滤 ======================= */
    private WorldPoints mapToWorldAndFilter(SensorPoints sp, double plcX) {
        final int n = sp.count;

        double[] Xw = new double[n];
        double[] Yw = new double[n];
        double[] Zw = new double[n];
        int valid = 0;

        final double[] R = cfg.R; // 行主序 R[0..8]

        // 变更点：CENTER 也允许融合，方向按左右侧确定：LEFT=-1，其余（RIGHT/CENTER）=+1
        final int sideSign = (cfg.side == MountSide.LEFT) ? -1 : +1;
        final boolean followPLC = cfg.plcMergeEnabled; // 是否跟随 PLC 仅由配置控制

        for (int i = 0; i < n; i++) {
            // 1) 读取原始（传感器系，mm）
            double xs = sp.xs[i], ys = sp.ys[i], zs = sp.zs[i];

            // 2) 符号位/翻转/比例
            double lx = xs * cfg.sX * (cfg.invertX ? -1.0 : 1.0) * cfg.xScale;
            double ly = ys * cfg.sY * (cfg.invertY ? -1.0 : 1.0) * cfg.yScale;
            double lz = zs * cfg.sZ * (cfg.invertZ ? -1.0 : 1.0) * cfg.zScale;

            // 3) 安装姿态旋转：v' = R * v
            final double rx = R[0]*lx + R[1]*ly + R[2]*lz;
            final double ry = R[3]*lx + R[4]*ly + R[5]*lz;
            final double rz = R[6]*lx + R[7]*ly + R[8]*lz;

            // 4) 平移/基高
            double xLocal = rx;
            double yWorld = ry + cfg.yOffset;          // Y 平移
            double zWorld = rz + cfg.h0 + cfg.zOffset; // Z 叠加 h0 与 zOffset

            // 5) PLC-X 融合（由开关决定；CENTER 不再强制关闭）
            double xWorld;
            if (followPLC) {
                xWorld = plcX + sideSign * xLocal + cfg.xOffset; // LEFT 取减，其余加
            } else {
                xWorld = xLocal + cfg.xOffset;                   // 固定安装或显式关闭
            }

            // 6) 过滤（世界系）—— ✅ 加入 X 过滤
            if (xWorld >= cfg.xMin && xWorld <= cfg.xMax &&
                    yWorld >= cfg.yMin && yWorld <= cfg.yMax &&
                    zWorld >= cfg.zMin && zWorld <= cfg.zMax) {
                Xw[valid] = xWorld;
                Yw[valid] = yWorld;
                Zw[valid] = zWorld;
                valid++;
            }
        }

        if (valid == 0) return new WorldPoints(new double[0], new double[0], new double[0], 0);
        if (valid < n) {
            Xw = Arrays.copyOf(Xw, valid);
            Yw = Arrays.copyOf(Yw, valid);
            Zw = Arrays.copyOf(Zw, valid);
        }
        return new WorldPoints(Xw, Yw, Zw, valid);
    }

    /* ======================= 推送上层 ======================= */
    private void pushFrame(long hostNs, WorldPoints wp) {
        if (listener != null && wp.valid > 0) {
            listener.onFrame(new ScanFrame(hostNs, null, wp.Xw, wp.Yw, wp.Zw));
        }
    }

    /* ======================= 原始持久化（保留） ======================= */
    private void initializePersistence() {
        try {
            Path outputPath = Paths.get(OUTPUT_DIR);
            Files.createDirectories(outputPath);
            log.info("[LIVOX] Raw persistence output directory: {}", outputPath.toAbsolutePath());
            switchOutputFile();
        } catch (IOException e) {
            log.warn("[LIVOX] Failed to initialize raw persistence directory: {}", e.getMessage());
            persistenceEnabled = false;
        }
    }
    private void switchOutputFile() {
        closeCurrentWriter();
        try {
            String timestamp = new SimpleDateFormat("yyyyMMdd_HHmmss_SSS").format(new Date());
            String filename = String.format("livox_raw_%s_%d.ply", timestamp, cfg.udpPort);
            Path filePath = Paths.get(OUTPUT_DIR, filename);
            currentWriter = new PrintWriter(new FileWriter(filePath.toFile()));
            currentFileStartTime.set(System.currentTimeMillis());
            pointCountInCurrentFile.set(0);
            writePlyHeader(currentWriter);
            log.info("[LIVOX] Created new raw output file: {}", filePath.getFileName());
        } catch (IOException e) {
            log.warn("[LIVOX] Failed to create raw output file: {}", e.getMessage());
            currentWriter = null;
        }
    }
    private void writePlyHeader(PrintWriter writer) {
        writer.println("ply");
        writer.println("format ascii 1.0");
        writer.println("comment Generated by LivoxMid360ScannerStrategy - RAW DATA");
        writer.println("comment Timestamp: " + new Date());
        writer.println("comment Data: Raw sensor coordinates (millimeters) with intensity");
        writer.println("comment Coordinate system: Livox sensor coordinate system");
        writer.println("element vertex 0");
        writer.println("property float x");
        writer.println("property float y");
        writer.println("property float z");
        writer.println("property int intensity");
        writer.println("property int packet_id");
        writer.println("property double timestamp_ns");
        writer.println("property double packet_timestamp_ns");
        writer.println("end_header");
    }
    private void persistRawPointsToFile(SensorPoints sp, long hostNs, long packetId) {
        long now = System.currentTimeMillis();
        if (currentWriter == null || (now - currentFileStartTime.get()) >= FILE_SWITCH_INTERVAL_MS) {
            switchOutputFile();
        }
        if (currentWriter != null) {
            try {
                for (int i = 0; i < sp.count; i++) {
                    currentWriter.printf(Locale.US, "%.3f %.3f %.3f %d %d %.0f %.0f\n",
                            sp.xs[i], sp.ys[i], sp.zs[i], sp.intensities[i], packetId,
                            (double) hostNs, (double) sp.packetTimestamp);
                    pointCountInCurrentFile.incrementAndGet();
                }
                if (pointCountInCurrentFile.get() % 1000 == 0) currentWriter.flush();
            } catch (Exception e) {
                log.warn("[LIVOX] Failed to write raw points: {}", e.getMessage());
            }
        }
    }
    private void closeCurrentWriter() {
        if (currentWriter != null) {
            try {
                if (pointCountInCurrentFile.get() > 0) {
                    currentWriter.close();
                    String currentFileName = getCurrentFileName();
                    if (currentFileName != null) {
                        Path filePath = Paths.get(OUTPUT_DIR, currentFileName);
                        String content = new String(Files.readAllBytes(filePath));
                        content = content.replaceFirst("element vertex 0",
                                "element vertex " + pointCountInCurrentFile.get());
                        Files.write(filePath, content.getBytes());
                        log.info("[LIVOX] Closed raw data file: {} ({} points)",
                                currentFileName, pointCountInCurrentFile.get());
                    }
                } else {
                    currentWriter.close();
                }
            } catch (Exception e) {
                log.warn("[LIVOX] Error closing raw output file: {}", e.getMessage());
            } finally {
                currentWriter = null;
            }
        }
    }
    private String getCurrentFileName() {
        try {
            return Files.list(Paths.get(OUTPUT_DIR))
                    .filter(path -> path.toString().endsWith(".ply"))
                    .max((p1, p2) -> {
                        try {
                            return Files.getLastModifiedTime(p1).compareTo(Files.getLastModifiedTime(p2));
                        } catch (IOException e) { return 0; }
                    })
                    .map(Path::getFileName)
                    .map(Path::toString)
                    .orElse(null);
        } catch (IOException e) { return null; }
    }

    /* ======================= 工具/内类 ======================= */
    private void hexdump(byte[] a, int len, int max) {
        int n = Math.min(len, max);
        StringBuilder sb = new StringBuilder();
        for (int i = 0; i < n; i++) sb.append(String.format("%02X ", a[i]));
        log.info("[LIVOX] head {}B: {}", n, sb.toString());
    }

    private static final class SensorPoints {
        final double[] xs, ys, zs;
        final int[] intensities;
        final int count;
        final long packetTimestamp;
        SensorPoints(double[] xs, double[] ys, double[] zs, int[] intensities, int count, long packetTimestamp) {
            this.xs = xs; this.ys = ys; this.zs = zs;
            this.intensities = intensities; this.count = count; this.packetTimestamp = packetTimestamp;
        }
        @Override public String toString() { return "SensorPoints{count=" + count + ", ts=" + packetTimestamp + "}"; }
    }

    private static final class WorldPoints {
        final double[] Xw, Yw, Zw; final int valid;
        WorldPoints(double[] Xw, double[] Yw, double[] Zw, int valid) {
            this.Xw = Xw; this.Yw = Yw; this.Zw = Zw; this.valid = valid;
        }
    }
}
